//////////////////////////////////////////////////////////////////
// MattiseOS Kernel
// Date: 2008
// Author: Matthew Iselin
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// 
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//////////////////////////////////////////////////////////////////

// Includes
#include <mattise.h>
#include <stdint.h>
#include <mem/util.h>

// Taken from the NEWLIB 1.15.0 sources

// sets memory to a certain value
#if !PREFER_SPEED
inline void* memset(const void* p, uint8_t val, size_t len)
{
	// create an 8-bit pointer
	uint8_t* dest = (uint8_t*) p;

	// fill it
	size_t i;
	for( i = 0; i < len; i++ )
		*dest++ = val;
	return (void*) p;
}
#else
#define LBLOCKSIZE (sizeof(long))
#define UNALIGNED(X)   ((long)X & (LBLOCKSIZE - 1))
#define TOO_SMALL(LEN) ((LEN) < LBLOCKSIZE)

inline void* memset(const void* m, unsigned char c, size_t n)
{
  char *s = (char *) m;
  unsigned int i;
  unsigned long buffer;
  unsigned long *aligned_addr;
  unsigned int d = c & 0xff;	/* To avoid sign extension, copy C to an
				   unsigned variable.  */

  if (!TOO_SMALL (n) && !UNALIGNED (m))
    {
      /* If we get this far, we know that n is large and m is word-aligned. */
      aligned_addr = (unsigned long*)m;

      /* Store D into each char sized location in BUFFER so that
         we can set large blocks quickly.  */
      if (LBLOCKSIZE == 4)
        {
          buffer = (d << 8) | d;
          buffer |= (buffer << 16);
        }
      else
        {
          buffer = 0;
          for (i = 0; i < LBLOCKSIZE; i++)
	    buffer = (buffer << 8) | d;
        }

      while (n >= LBLOCKSIZE*4)
        {
          *aligned_addr++ = buffer;
          *aligned_addr++ = buffer;
          *aligned_addr++ = buffer;
          *aligned_addr++ = buffer;
          n -= 4*LBLOCKSIZE;
        }

      while (n >= LBLOCKSIZE)
        {
          *aligned_addr++ = buffer;
          n -= LBLOCKSIZE;
        }
      /* Pick up the remainder with a bytewise loop.  */
      s = (char*)aligned_addr;
    }

  while (n--)
    {
      *s++ = (char)d;
    }

  return (void*) m;
}
#endif
